/*
 * CircularLoads.cpp
 *
 *      Author: Jerome RD Soine and Christoph A Brand
 *      Institution: Schwarz goup, Institute for Theoretical Physics, Heidelberg University, Germany
 */

#include "../../../../include/base/substrate_models/ElasticHalfspace/CircularLoads.h"
#include "../../../../include/base/utility/FieldInterpolation2D.h"
#include <cmath>
#include <fstream>

using namespace std;

CircularLoads::CircularLoads(double YOUNGSMOD, double POISSONRATIO,vector<ForceTransmission*> FORCE_TRANSMISSIONS)
{

		youngs_modulus = YOUNGSMOD;
		poisson_ratio=POISSONRATIO;
		force_trans=FORCE_TRANSMISSIONS;


		vector<double> x_range = ForceTransmission::get_x_range(force_trans);
		vector<double> y_range = ForceTransmission::get_y_range(force_trans);
		double diff = x_range[1]-x_range[0];
		x_range[0]-=diff*0.5;
		x_range[1]+=diff*0.5;
		diff = y_range[1]-y_range[0];
		y_range[0]-=diff*0.5;
		y_range[1]+=diff*0.5;
		// Calculate n_ref automatically
		diff = x_range[1]-x_range[0];
		double eff_mesh_size = sqrt(ForceTransmission::get_mean_area(force_trans))/8.0;
		unsigned int n_ref = (int)log2(diff/eff_mesh_size);
		interpolation = new FieldInterpolation2D(Point<2> (x_range[0],y_range[0]),Point<2> (x_range[1],y_range[1]),n_ref,this,force_trans);
	};

vector<double> CircularLoads::get_displacement_at (vector<double> point)
{
	vector<double> displacement (2,0);
	for(unsigned int i=0;i<force_trans.size();i++)
	{
		vector<double> tmp_disp = evaluate_single_load_displacement(point, force_trans[i]);
		displacement[0] += tmp_disp[0];
		displacement[1] += tmp_disp[1];
	}
	return displacement;
}

vector<vector<double> > CircularLoads::get_displacement_list_at(vector<vector<double> > points)
{
	vector<vector<double> > displacements (points.size());
	for(unsigned int i=0;i<points.size();i++)
	{
		displacements[i] = get_displacement_at(points[i]);
	}
	return displacements;
}

vector<double> CircularLoads::evaluate_single_load_displacement (vector<double> point, ForceTransmission* force_trans)
{

	vector<double> local_point = transform_to_local_coordidantes(point,force_trans);

	// Calculate
	vector<double> displacement (2,0);
	double distance = sqrt(local_point[0]*local_point[0] + local_point[1]*local_point[1]);
	double a = force_trans->get_shape()->get_radius();
	double force_mag = force_trans->get_force_magnitude();
	double traction_constant = calculate_traction_constant(force_mag,a);
	double x=local_point[0];
	double y=local_point[1];
	double radius = sqrt(x*x+y*y);
	// Halfspace Solution for r<=a
	if(distance<=force_trans->get_shape()->get_radius())
	{
		double prefactor = M_PI*traction_constant*2*(1+poisson_ratio)/(32*youngs_modulus*a);
		displacement[0] = prefactor*(4*(2-poisson_ratio)*a*a-(4-3*poisson_ratio)*x*x-(4-poisson_ratio)*y*y);
		displacement[1] = prefactor*2*poisson_ratio*x*y;
	}
	else
	{
		double prefactor = traction_constant*2*(1+poisson_ratio)/(8*youngs_modulus*a);
		displacement[0] = prefactor*(
					(2-poisson_ratio)*((2*a*a-radius*radius)*asin(a/radius)+a*radius*sqrt(1-a*a/radius/radius))
					+0.5*poisson_ratio*(x*x-y*y)*(radius*radius*asin(a/radius)+(2*a*a-radius*radius)*sqrt(1-a*a/radius/radius)*(a/radius))/radius/radius
					);
		displacement[1] = prefactor*poisson_ratio*x*y*(radius*radius*asin(a/radius)+(2*a*a-radius*radius)*sqrt(1-a*a/radius/radius)*(a/radius))/radius/radius;
	}

	// Rotate displacement back to global base
	vector<double> force = force_trans->get_force();
	double force_angle = atan2(force[1],force[0]);
	displacement = rotate_clockwise(displacement,force_angle);

	return displacement;
}

double CircularLoads::calculate_traction_constant (double force, double radius)
{
	// Calculate traction constant q_0 from relation F = int q_0(1-r^2/a^2)^(1/2) dA
	double value = 3*force/(2*M_PI*radius*radius);
	return value;
}

vector<double> CircularLoads::rotate_clockwise(vector<double> point, double angle)
{
	vector<double> rotated_point (2);
	rotated_point[0] = point[0]*cos(angle) - point[1]*sin(angle);
	rotated_point[1] = point[0]*sin(angle) + point[1]*cos(angle);
	return rotated_point;
}

vector<double> CircularLoads::rotate_counterclockwise(vector<double> point, double angle)
{
	vector<double> rotated_point (2);
	rotated_point[0] = point[0]*cos(angle) + point[1]*sin(angle);
	rotated_point[1] = - point[0]*sin(angle) + point[1]*cos(angle);
	return rotated_point;
}


vector<double> CircularLoads::transform_to_local_coordidantes (vector<double> point, ForceTransmission* force_trans)
{
	vector<double> center_global = force_trans->get_shape()->get_center();
	vector<double> force = force_trans->get_force();
	double force_angle = atan2(force[1],force[0]);

	// Translate global coordinate -> new origin is center of the circular shape
	point[0] = point[0] - center_global[0];
	point[1] = point[1] - center_global[1];

	// Rotate Coordinate System
	return rotate_counterclockwise (point,force_angle);
}

void CircularLoads::write_output(string filename, vector<vector<double> > points)
{
	ofstream out (filename.c_str());
	out << "# 1 x-pos <TAB> 2 y-pos <TAB> 3 x_displacement <TAB> y_displacement \n";
	for(unsigned int i=0;i<points.size();i++)
	{
		vector<double> disp = get_displacement_at(points[i]);
		out << points[i][0] << "\t" << points[i][1] << "\t" << disp[0] << "\t" << disp[1] << "\n";
	}
	out.close();
}

void CircularLoads::write_forces(string filename)
{
	ofstream out (filename.c_str());
	out << "# 1 x-pos <TAB> 2 y-pos <TAB> 3 x_traction <TAB> y_traction <TAB> 4 tractionmagnitude \n";
	for(unsigned int i=0;i<force_trans.size();i++)
	{
		out << force_trans[i]->get_shape()->get_center()[0] << "\t"
			<< force_trans[i]->get_shape()->get_center()[1] << "\t"
			<< force_trans[i]->get_force()[0] << "\t"
			<< force_trans[i]->get_force()[1] << "\t"
			<< force_trans[i]->get_force_magnitude() << "\n";
	}
	out.close();
}


void CircularLoads::write_output(string filename)
{
	// Calculate ranges
	interpolation->interpolate_fields(this,force_trans);
	// Write displacement file
    interpolation->output((filename+"_disp.vtk"));
    interpolation->output_force((filename+"_force.vtk"));
}

